#include <avr/io.h>

#include "rot_tsetups.h"
#include "rot_servo.h"

volatile uint16_t TASK1_CNT;
volatile uint16_t TASK2_CNT;

volatile uint8_t TASK1_FLAG;
volatile uint8_t TASK2_FLAG;

void timer1_init(){

  //disable all interupts
  TIMSK1 &= ~( _BV(TOIE1) | _BV(ICIE1) | _BV(OCIE1A) | _BV(OCIE1B));
  //set timer mode
  TCCR1A &= ~( _BV(WGM11) | _BV(WGM10) );
  TCCR1B &= ~( _BV(WGM12) | _BV(WGM13) | _BV(ICNC1));
  //capture raising edge
  TCCR1B |= _BV(ICES1);
  //prescaler 1/8
  TCCR1B |= _BV(CS11);
  TCCR1B &= ~( _BV(CS12) | _BV(CS10) );
  //disable outputs
  TCCR1A &= ~( _BV(COM1A0) | _BV(COM1A1) | _BV(COM1B0) | _BV(COM1B1));
  //enable capture interupt
  TIMSK1 |= (1<<ICIE1);
  //set ICP1 as input
  DDRB &= ~(1<<DDB0);
  PORTB |= (1<<PORTB0);
  MCUCR &= ~(1<<PUD);

}


void timer2_init(void){

  /* No pre-scaler (16Mhz) */
  TCCR2B |= _BV(CS20);
  /* Enable Timer Overflow Interrupts */
  TIMSK2 |= _BV(TOIE2);
  /* Reset Counters */
  TCNT2 = 0;
  H_COUNTER2 = 0;
  task_flags_init();

}

void task_flags_init(void){

  TASK1_CNT = 0;
  TASK2_CNT = 0;
  TASK1_FLAG = 0;
  TASK2_FLAG = 0;

}

void manage_tasks(void){

  TASK1_CNT++;
  TASK2_CNT++;
  if (TASK1_CNT>TASK1_T) {
    TASK1_FLAG = 1;
    TASK1_CNT = 0;
  }
  if (TASK2_CNT>TASK2_T) {
    TASK2_FLAG = 1;
    TASK2_CNT = 0;
  }

}
